#line 1 "D:/Archivio/mikroC/tric-newsensors/tric-newsensors.c"
#line 1 "d:/archivio/mikroc/tric-newsensors/lib_bma180.h"
#line 8 "d:/archivio/mikroc/tric-newsensors/lib_bma180.h"
void BMA180_init();
void BMA180_readAcc();
#line 1 "d:/archivio/mikroc/tric-newsensors/lib_itg3200.h"
#line 8 "d:/archivio/mikroc/tric-newsensors/lib_itg3200.h"
void ITG3200_init();
void ITG3200_readGyro();
#line 1 "d:/archivio/mikroc/tric-newsensors/tric-newsensors.h"



 void Soft_Uart_Write_Text(char* testo);
 void main();
#line 21 "D:/Archivio/mikroC/tric-newsensors/tric-newsensors.c"
int acc[3];
int gyro[3];
float accf[3];
float tau=0.05;
float aa=0.0;
float dt=0.015;
unsigned short RX_values[4];
 float modulo, roll, pitch, yaw;
#line 32 "D:/Archivio/mikroC/tric-newsensors/tric-newsensors.c"
void Soft_UART_Write_Text(char* testo) {
 char cnt;
 for (cnt=0; cnt<strlen(testo); cnt++) {
 Soft_UART_Write(testo[cnt]);
 }
}

char *strstrip(char *s)
{
 char *end;
 end = s + strlen(s) - 1;
 while (end >= s && isspace(*end))
 end--;
 *(end + 1) = '\0';

 while (*s && isspace(*s))
 s++;

 return s;
}
#line 57 "D:/Archivio/mikroC/tric-newsensors/tric-newsensors.c"
int PID_update() {
 static float PID_err_proportional_old[2];
 static float PID_err_integral[2];
 float PID_err_proportional[2];
 float PID_err_derivative[2];
 int PID_output[2];
 unsigned short cnt;

 cnt=0;
 RX_values[0]=0;


 PID_err_proportional[cnt] = RX_values[cnt] - roll;
 if((PID_err_proportional[cnt] >  2 ) || (PID_err_proportional[cnt] < - 2 )) PID_err_integral[cnt] += PID_err_proportional[cnt]* dt;
 PID_err_integral[cnt] += PID_err_proportional[cnt] * dt;
 PID_err_derivative[cnt] = (PID_err_proportional[cnt] - PID_err_proportional_old[cnt]) / dt;
 PID_output[cnt] = (int)( ( 0.7 *PID_err_proportional[cnt]) + ( 0.2 *PID_err_integral[cnt]) + ( 0.01 *PID_err_derivative[cnt]) );

 if(PID_output[cnt] >  255 ) PID_output[cnt] =  255 ;
 else if(PID_output[cnt] <  0 ) PID_output[cnt] =  0 ;

 PID_err_proportional_old[cnt] = PID_err_proportional[cnt];

 return PID_output[0];
}

float Complementary(float oldAngle, float newAngle, float newRate) {
 aa=tau/(tau+dt);

 oldAngle= aa* (oldAngle + newRate * dt) + (1-aa) * (newAngle);
 return oldAngle;
}

void main() {
 char x_string[8];
 char y_string[8];
 char z_string[8];
 char v_string[8];

  TRISC.B2  = 0;

 ADCON1 = 0x0F;
 CMCON = 0xCF;

 Soft_UART_Init(&PORTB, 2, 3, 115200, 0);

 Delay_ms(500);
 I2C1_Init(400000);
 Delay_ms(500);
  PORTC.B2 =1;

 BMA180_init();
 Delay_ms(500);
 ITG3200_init();
 Delay_ms(500);
  PORTC.B2 =0;

 while (1) {
  PORTC.B2  = ! PORTC.B2 ;

 BMA180_readAcc();
 ITG3200_readGyro();

 gyro[0] = gyro[0] / 14.375;
 gyro[1] = gyro[1] / 14.375;
 gyro[2] = gyro[2] / 14.375;

 accf[0] = acc[0] / 4096.0;
 accf[1] = acc[1] / 4096.0;
 accf[2] = acc[2] / 4096.0;

 modulo = sqrt(accf[0]*accf[0]+accf[1]*accf[1]+accf[2]*accf[2]);
 roll = Complementary(roll, asin(accf[1]/modulo)*57.295779, -gyro[0]);
 pitch = Complementary(pitch, asin(accf[0]/modulo)*57.295779, gyro[1]);
 yaw += (gyro[2]*dt);

 RX_values[0] = 0;

 IntToStr(PID_update(), v_string);
 IntToStr(floor(roll), x_string);
 IntToStr(floor(asin(accf[1]/modulo)*57.295779), y_string);
 IntToStr(floor(yaw), z_string);


 Soft_UART_Write_Text("100,");
 Soft_UART_Write_Text(&x_string);
 Soft_UART_Write_Text(",");
 Soft_UART_Write_Text(&y_string);
 Soft_UART_Write_Text(",");
 Soft_UART_Write_Text(&z_string);
 Soft_UART_Write_Text(",");
 Soft_UART_Write_Text(&v_string);

 Soft_UART_Write(10);
#line 165 "D:/Archivio/mikroC/tric-newsensors/tric-newsensors.c"
 Delay_ms(20);
 }
}
